#include <Servo.h> 
 
Servo myservoAvantBras;  // create servo object to control a servo 
                // a maximum of eight servo objects can be created 

Servo myservoBras;  // create servo object to control a servo 

 
int pos = 0;    // variable to store the servo position 

// nombre de ms pour parcourrir un degré d'angle
// 0.12s pour 60° à 4.8V
int vitesseAngulaire = 2;
int angle = 0; 

char buffer[20];

//int angleMin = 12;
//int angleMax = 145;
int angleMin = 5;
int angleMax = 360;

int oldAngleBras = 90;
int oldAngleAvantBras = 90;

//int ReceptionNombre=0; // variable de calcul du nombre reçu par port série
void setup() 
{ 
  myservoAvantBras.attach(7);  // attaches the servo on pin 9 to the servo object 
  myservoBras.attach(8);  // attaches the servo on pin 9 to the servo object 
  Serial.begin(115200);  
  myservoAvantBras.write(oldAngleAvantBras);
  myservoBras.write(oldAngleBras);
} 

int octetReception=0; // variable de stockage des valeurs reçues sur le port Série
long nombreReception=0; // variable de stockage du nombre  reçu sur le port Série
long nombreReception0=0; // variable de stockage du dernier nombre  reçu sur le port Série
String chaineReception=""; // déclare un objet String vide pour reception chaine
 
char chaine[255]; 
 
/* La vitesse donnée est un entier compris entre 0 (tres lente) et 255 (vitesse maximale sans palliers) */
void move( Servo * servo, int * oldAngle, int newAngle, int speed )
{
  char chaine[255];
  sprintf(chaine, "moving from %d to %d", *oldAngle, newAngle );
  Serial.println( chaine );
  if ( speed == 255 )
  {
      servo->write( newAngle );
  }
  else
  {
    if ( *oldAngle > newAngle )
    {
      for ( int i = *oldAngle ; i > newAngle ; i--)
      {
        servo->write( i );
        delay( (int) vitesseAngulaire * (255 - speed) );
      }
    }
    else if ( *oldAngle < newAngle )
    {
      for ( int i = *oldAngle; i < newAngle ; i ++)
      {
        servo->write( i );
        delay( (int) vitesseAngulaire * (255 - speed));
      }
    }
  }
  *oldAngle = newAngle;

}
 
void loop() 
{ 

  if (Serial.available()>0) 
  {
//    chaineReception= "";
    int i=0;
    while ( (octetReception = Serial.read() ) != 10) 
    { // tant qu'un octet en réception 
//      chaineReception += octetReception; 
    Serial.println( octetReception );
      chaine[i] = octetReception;
      i++;
    }
    
    chaine[i]='\0';
    Serial.println( "fin capture chaine" );
    Serial.println( chaine );
    if ( strcmp( chaine,"bye" ) == 0 )
    {
      Serial.println( "début chorégraphie" );
      move( &myservoBras, &oldAngleBras, 120, 250 );
      for (int i = 0; i < 3; i ++)
      {
        Serial.println( "début" );
        move( &myservoAvantBras, &oldAngleAvantBras, 60, 250 );
        Serial.println( "Milieu" );
        move( &myservoAvantBras, &oldAngleAvantBras, 130, 250 );
        Serial.println( "Fin" );
      }
      move( &myservoBras, &oldAngleBras, 50, 250 );

    }

    if ( strcmp( chaine,"karate" ) == 0 )
    {
      Serial.println( "début chorégraphie" );
      move( &myservoAvantBras, &oldAngleAvantBras, 80, 250  );
      move( &myservoBras, &oldAngleBras, 150, 250 );

      move( &myservoBras, &oldAngleBras, 20, 255 );
      delay(5000);
      Serial.println( "Fin" );
      move( &myservoBras, &oldAngleBras, 50, 250 );
    }

    if ( strcmp( chaine,"rangement" ) == 0 )
    {
      Serial.println( "début chorégraphie" );
      move( &myservoAvantBras, &oldAngleAvantBras, 5, 250 );
      move( &myservoBras, &oldAngleBras, 20, 250 );

      Serial.println( "Fin" );
    }

    
    chaineReception = "";

//    delay(angle * vitesse_angulaire);
  }
} 
